#!/usr/bin/python

import roslib; roslib.load_manifest('framer')
import rospy
import smach
import smach_ros
from moveHead      import MoveHead
from moveBase      import MoveBase
from pictureTaker  import PictureTaker
from faceProcessor import FaceProcessor
from ruleOfThirds import RuleOfThirds
from centerRule import CenterRule
from interact import RequestInteraction
from idleState import IdleState


if __name__ == '__main__':
    rospy.init_node('framer')
   
    machine = smach.StateMachine(outcomes=['success', 'fail', 'aborted']) # yes, nothing links to success or fail. Smach doesn't care
    with machine:
        smach.StateMachine.add('Idle State', IdleState(),transitions={
            'idle':    'Idle State',
            'prepare': 'Look for Targets'
        })
        smach.StateMachine.add('Look for Targets', MoveHead(),
            transitions={
                'success':'Face Processor',
                'fail':'Idle State',
                'abort':'aborted'
            }
        )
        smach.StateMachine.add('Face Processor', FaceProcessor(),
            transitions={'next':'Raise Camera', 'abort':'aborted'}
        )
        smach.StateMachine.add('Move Head', MoveHead(),
            transitions={
                'success':'Picture Taker',
                'fail':'Lower Camera',
                'abort':'aborted'
            }
        )
        smach.StateMachine.add("Rule of Thirds", RuleOfThirds(),
            transitions={
                'success':  'Move Base',
                'fail':     'Lower Camera',
                'abort':    'aborted',
                'request':  'Request Interaction',
                'center':   'Center Rule',
            }
        )
        smach.StateMachine.add("Center Rule", CenterRule(),
            transitions={
                'success':  'Move Head',
                'fail':     'Lower Camera',
                'abort':    'aborted',
            }
        )
        smach.StateMachine.add("Move Base", MoveBase(),
            transitions={
                'success':'Move Head',
                'fail':'Lower Camera',
                'abort':'aborted'
            }
        )
        smach.StateMachine.add('Picture Taker', PictureTaker(),
            transitions={
                'success':'Lower Camera',
                'abort':'aborted',
                'fail':'Lower Camera',
                'request':'Request Interaction'
            }
        )

        smach.StateMachine.add('Raise Camera', RequestInteraction('raise_camera_to_eye'),
            transitions={
                'success': 'Rule of Thirds',
                'fail':   'Face Processor',
            }
        )

        smach.StateMachine.add('Lower Camera', RequestInteraction('lower_camera_from_eye'),
            transitions={
                'success':  'Idle State',
                'fail':     'Idle State',
            }
        )

        smach.StateMachine.add('Request Interaction', RequestInteraction(),
            transitions={
                'success': 'Lower Camera',
                'fail':   'Lower Camera',
            }
        )

    try:
        sis = smach_ros.IntrospectionServer('framer', machine, '/FRAMER_ROOT')
        sis.start()
        rospy.loginfo("Starting framer state machine.")
        machine.execute()
        rospy.spin()
    except KeyboardInterrupt:
        rospy.loginfo("Shutting down")
